#include "bonderhead.h"

SILICOOL_CREATE_LOGGING_CATEGORY(bondingTimeLog, "bondingTimeLog")

void SoftLandingPickDownCmd::runImpl()
{
    bh->pickDutDownAction();
}

void PerformUplookPrAndCorrectTargetPosCmd::resetCmdResult()
{
    m_cmdResult = false;
}

void PerformUplookPrAndCorrectTargetPosCmd::runImpl()
{
    PrOffset lensUplookPrOffset;
    if (bh->getLensUplookOffsetImpl(lensUplookPrOffset))
    {
        PrOffset sensorPrOffset = bh->bwh->getLastSensorOffset();
        QPoint sensorIndex = bh->bwh->getLastSensorIndex();
        double targetYOffset = sensorPrOffset.Y + bh->bwhOpticXY->yAxis()->getFeedbackPos(-1);
        auto cameraToUplookCameraOffset
            = bh->bhConfig->stCameraToUplookCameraOffsetOfEachRow()->getConfig<CameraOffset>(sensorIndex.x());
        TwoDimensionCalibration *tdc
            = bh->bhConfig->useStSideTwoDimCaliData() ? bh->stSideTdc(sensorIndex.x()) : nullptr;
        bh->fastMoveToStCameraPlacePos(lensUplookPrOffset, cameraToUplookCameraOffset,
                                       bh->bhConfig->useCorFromCalibration(), sensorPrOffset.X, targetYOffset,
                                       sensorPrOffset.Theta, tdc);
        m_cmdResult = true;
    }
    else
    {
        m_cmdResult = false;
    }
}

BonderHead::BonderHead(TrayConfig *sensorTrayConfig,
                       TrayConfig *lensTrayConfig,
                       BonderWorkHolder *bwh,
                       LensTrayLoader *ltLoader,
                       UPHHelper *uphHelper)
    : Worker(WorkerName::BonderHead),
      sensorTrayConfig(sensorTrayConfig),
      lensTrayConfig(lensTrayConfig),
      bwh(bwh),
      ltLoader(ltLoader),
      uphHelper(uphHelper)
{
    bhConfig = new BonderHeadConfig();
    pickPlaceConfig = bhConfig->pickPlaceConfig();
    bhConfigFile
        = new ConfigFile("BonderHeadConfig", bhConfig, QString("%1/BonderHeadConfig.json").arg(getWorkerConfigDir()));
    bhConfigFile->populate();

    yzPlanner = new TwoAxisPlanner();
    zyPlanner = new TwoAxisPlanner();

    thdPool = new QThreadPool(this);
    thdPool->setMaxThreadCount(1);
    thdPool->setExpiryTimeout(-1);
    softLandingPickDownCmd = new SoftLandingPickDownCmd(this);
    softLandingPickDownCmd->setAutoDelete(false);
    performUplookPrAndCorrectTargetPosCmd = new PerformUplookPrAndCorrectTargetPosCmd(this);
    performUplookPrAndCorrectTargetPosCmd->setAutoDelete(false);
}

BonderHead::~BonderHead()
{
    freeTdcDatas();
    delete softLandingPickDownCmd;
    delete performUplookPrAndCorrectTargetPosCmd;
}

void BonderHead::initEventHandle()
{
    Calibration *ltCaliGlassCalibration = VisionManager::getIns().getCalibration(CaliName::LTCaliGlassCalibration);
    ltCaliGlassCalibration->setCustomFunc(
        std::bind(&BonderHead::ltCaliGlassCustomCaliFunc, this, std::placeholders::_1, std::placeholders::_2,
                  std::placeholders::_3, std::placeholders::_4),
        std::bind(&BonderHead::ltCaliGlassPerformPrResultFunc, this, std::placeholders::_1));

    Calibration *stCaliGlassCalibration = VisionManager::getIns().getCalibration(CaliName::STCaliGlassCalibration);
    stCaliGlassCalibration->setCustomFunc(
        std::bind(&BonderHead::stCaliGlassCustomCaliFunc, this, std::placeholders::_1, std::placeholders::_2,
                  std::placeholders::_3, std::placeholders::_4),
        std::bind(&BonderHead::stCaliGlassPerformPrResultFunc, this, std::placeholders::_1));

    bhZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::BondHeadZ);
    bhXyz = MotionElementContainer::getIns()->getItem<XYZModule>(XYZModuleName::BondHeadXYZ);
    bhTheta = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::BondHeadTheta);
    bhMiniZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::BondHeadMiniZ);
    bhVacuum = MotionElementContainer::getIns()->getItem<SCVacuum>(VacuumName::BondHeadVacuum);
    bhMiniZMotor = MotionElementContainer::getIns()->getItem<SCAxis>(AxisName::BHMiniZ);
    bhThetaMotor = MotionElementContainer::getIns()->getItem<SCAxis>(AxisName::BHTheta);
    ltLoaderTrackX = MotionElementContainer::getIns()->getItem<SCAxis>(AxisName::LTLoaderTrackX);

    bwhOpticXY = MotionElementContainer::getIns()->getItem<XYModule>(XYModuleName::BWHOpticXY);
    bwhOpticZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::BWHOpticZ);

    yzPlanner->init(bhXyz->yAxis(), bhXyz->zAxis());
    zyPlanner->init(bhXyz->zAxis(), bhXyz->yAxis());

    ltCaliGlassLocation = VisionManager::getIns().getVisionLocation(VLName::LTCaliGlassLocation);
    stCaliGlassLocation = VisionManager::getIns().getVisionLocation(VLName::STCaliGlassLocation);
    caliGlassUplookLocation = VisionManager::getIns().getVisionLocation(VLName::CaliGlassUplookLocation);
    lensUplookRectLocation = VisionManager::getIns().getVisionLocation(VLName::LensUplookRectLocation);
    lensUplookCircleLocation = VisionManager::getIns().getVisionLocation(VLName::LensUplookCircleLocation);

    loadTdcDatas();
}

void BonderHead::startEventHandle(const WorkerEvent &event)
{
    Q_UNUSED(event)
    isBwhStopped = false;
    pushEvent(WorkerEvent("startRun"));
}

void BonderHead::allWorkerStopped()
{
    isBwhStopped = false;
}

void BonderHead::purgeEventHandle()
{
    purgeEventHandled();
}

void BonderHead::resetEventHandle()
{
    isBwhStopped = false;
}

void BonderHead::homeWorker(bool moveToReadyPosAfterHome)
{
    bhXyz->zAxis()->home();
    QThread::msleep(100);
    bhXyz->zAxis()->absMove(bhXyz->zAxis()->positiveLimit());
    bhMiniZMotor->home();
    QThread::msleep(100);
    bhMiniZMotor->absMove(bhMiniZMotor->negativeLimit());

    bhXyz->xAxis()->home(false);
    bhXyz->yAxis()->home(false);
    bhXyz->xAxis()->waitHomeDone();
    bhXyz->yAxis()->waitHomeDone();

    if (moveToReadyPosAfterHome)
    {
        QThread::msleep(100);
        moveToStandbyPos(false);
    }
}

void BonderHead::startRun()
{
    resetBHYZAcc();
    moveToStandbyPos(true);
    pushGetSensorEventToBWH();
    pushPickLensEvent();
}

void BonderHead::waitLens()
{
    SCTimer t(__func__, bondingTimeLog());

    QElapsedTimer timer;
    timer.start();
    while (!ltLoader->gotLens())
    {
        if (hasAbortEvent())
        {
            return;
        }
        if (isBwhStopped)
        {
            handleStop();
            return;
        }
        if (timer.elapsed() > 10000)
        {
            qCInfo(logCate()) << tr("Waiting lens pr...");
            timer.restart();
        }
        QThread::msleep(10);
    }
}

void BonderHead::directMoveToPickLensPos()
{
    SCTimer t(__func__, bondingTimeLog());

    checkLensIndex();
    PrOffset lensPrOffset = ltLoader->getLastLensOffset();
    directMoveToPickLensPosImpl(lensPrOffset);
}

void BonderHead::pickLens()
{
    if (isBwhStopped)
    {
        handleStop();
        return;
    }
    try
    {
        {
            SCTimer t("pickDutDown", bondingTimeLog());
            pickDutDown();
        }

        {
            SCTimer t("softlandUpMiniZ", bondingTimeLog());
            softlandUpMiniZWithoutWait();
        }

        resetBHYZAcc();
        bhXyz->moveZToPos(XYZPosName::UplookPos, false);
        checkDutAfterPick();
        pickedLensIndex = ltLoader->getLastLensIndex();
        ltLoader->setLensState(pickedLensIndex, LensTrayMaterialData::InUse);
        pushEvent(WorkerEvent("moveToUplookPos"));
        return;
    }
    catch (SilicoolException &se)
    {
        qCWarning(logCate()) << se.what();
        qCWarning(logCate()) << tr("Pick lens failed! Try to pick next lens!");
        ltLoader->setLensState(ltLoader->getLastLensIndex(), LensTrayMaterialData::PickFailed);
        auto *standbyPos = bhXyz->getPos<XYZModulePos>(XYZPosName::StandbyPos);
        bhXyz->zAxis()->absForceMove(standbyPos->zPos());
        bhXyz->zAxis()->waitArrivedPos(standbyPos->zPos(), 0.002);
        bhXyz->moveXYToPos(XYZPosName::StandbyPos);
        pushPickLensEvent();
        return;
    }
}

void BonderHead::moveToUplookPos()
{
    {
        SCTimer t(__func__, bondingTimeLog());
        moveFromLtToUplookPosImpl();
    }

    if (!bhVacuum->get())
    {
        qCWarning(logCate()) << tr(
            "Bond head vacuum lost feedback signal after move to uplook pos! Try to pick next lens!");
        bhVacuum->set(false);
        ltLoader->setLensState(pickedLensIndex, LensTrayMaterialData::Lost);
        pushPickLensEvent();
        return;
    }

    pushEvent(WorkerEvent("waitSensor"));
    pushEvent(WorkerEvent("performUplookPrAndMoveToBondPos"));
}

void BonderHead::waitSensor()
{
    SCTimer t(__func__, bondingTimeLog());

    QElapsedTimer timer;
    timer.start();
    while (!bwh->gotSensor())
    {
        if (hasAbortEvent())
        {
            return;
        }
        if (isBwhStopped)
        {
            PrOffset lastLensOffset = ltLoader->getLastLensOffset();
            directMoveToPlacebackLensPos(lastLensOffset);
            placeDut(false);
            ltLoader->setLensState(pickedLensIndex, LensTrayMaterialData::Init);
            handleStop();
            return;
        }
        if (timer.elapsed() > 10000)
        {
            qCInfo(logCate()) << tr("Waiting bond work holder sensor pr...");
            timer.restart();
        }
        QThread::msleep(10);
    }
}

void BonderHead::performUplookPrAndMoveToBondPos()
{
    checkSensorIndex();
    pushGetLensEventToLTL();

    {
        SCTimer t("alignLensSensorAngle", bondingTimeLog());
        if (!bhConfig->useCorFromCalibration())
        {
            PrOffset lensUplookOffset;
            if (!lensUplookRectLocation->performPR(lensUplookOffset))
            {
                handleUplookPrFailed();
                return;
            }
            PrOffset sensorPrOffset = bwh->getLastSensorOffset();
            auto cameraToUplookCameraOffset
                = bhConfig->stCameraToUplookCameraOffsetOfEachRow()->getConfig<CameraOffset>(
                    bwh->getLastSensorIndex().x());
            double thetaAngle = sensorPrOffset.Theta - (cameraToUplookCameraOffset->theta() - lensUplookOffset.Theta);
            bhThetaMotor->relMove(thetaAngle, true, 0.001);
        }
    }

    {
        SCTimer t("triggerLensUplookPr", bondingTimeLog());
        performUplookPrAndCorrectTargetPosCmd->reset();
        performUplookPrAndCorrectTargetPosCmd->resetCmdResult();
        thdPool->start(performUplookPrAndCorrectTargetPosCmd);
        while (true)
        {
            QThread::msleep(1);
            if (performUplookPrAndCorrectTargetPosCmd->isStarted())
            {
                break;
            }
        }
        QThread::msleep(bhConfig->delayAfterGiveUplookPrCmd());    // wait triggering getting image
    }

    PrOffset sensorPrOffset = bwh->getLastSensorOffset();
    QPoint sensorIndex = bwh->getLastSensorIndex();
    double targetYOffset = sensorPrOffset.Y + bwhOpticXY->yAxis()->getFeedbackPos(-1);
    auto cameraToUplookCameraOffset
        = bhConfig->stCameraToUplookCameraOffsetOfEachRow()->getConfig<CameraOffset>(sensorIndex.x());
    auto uplookPos = bhXyz->getPos<XYZModulePos>(XYZPosName::UplookPos);
    double estimatedTargetYPos = uplookPos->yPos() + cameraToUplookCameraOffset->y() + targetYOffset;

    {
        SCTimer t("moveToEstimatedPos_WaitUplookPr_CorrectTargetPos", bondingTimeLog());

        bhXyz->yAxis()->absMove(estimatedTargetYPos, false);
        while (!performUplookPrAndCorrectTargetPosCmd->isFinished())
        {
            QThread::msleep(3);
        }
    }

    if (!performUplookPrAndCorrectTargetPosCmd->errMsg().isEmpty())
    {
        throw SilicoolException(performUplookPrAndCorrectTargetPosCmd->errMsg());
    }
    if (!performUplookPrAndCorrectTargetPosCmd->cmdResult())
    {
        handleUplookPrFailed();
        return;
    }

    pushEvent(WorkerEvent("bond"));
}

void BonderHead::bond()
{
    if (pickPlaceConfig->delayBeforePlace() > 0)
    {
        SCTimer t("delayBeforePlace", bondingTimeLog());
        QThread::msleep(pickPlaceConfig->delayBeforePlace());
    }
    try
    {
        SCTimer t("softLandPlaceDown", bondingTimeLog());
        placeDutDownAction();
    }
    catch (SilicoolException &se)
    {
        softLandUpTwoZ();
        throw se;
    }

    if (pickPlaceConfig->delayAfterSoftlandContact() > 0)
    {
        SCTimer t("delayAfterSoftlandContact", bondingTimeLog());
        QThread::msleep(pickPlaceConfig->delayAfterSoftlandContact());
    }

    QPoint sensorIndex = bwh->getLastSensorIndex();
    bwh->setSensorState(sensorIndex, SensorTrayMaterialData::Bonded);
    // Todo Cure
    bwh->setSensorState(sensorIndex, SensorTrayMaterialData::Cured);

    {
        SCTimer t("closeVacuum", bondingTimeLog());
        bhVacuum->set(false);
    }

    if (pickPlaceConfig->delayAfterCloseVacuum() > 0)
    {
        SCTimer t("delayAfterCloseVacuum", bondingTimeLog());
        QThread::msleep(pickPlaceConfig->delayAfterCloseVacuum());
    }

    {
        SCTimer t("softlandUpMiniZ", bondingTimeLog());
        softlandUpMiniZWithoutWait();
    }

    uphHelper->appendData();
    setHigherBHYZAcc();
    bhXyz->moveZToPos(XYZPosName::StandbyPos, false);
    pushEvent(WorkerEvent("postBond"));
}

void BonderHead::postBond()
{
    bwh->pushEvent(WorkerEvent("upOpticZ"));
    QVariantList args;
    args << pickedLensIndex.x() << pickedLensIndex.y();
    WorkerEvent bwhPostBondEvent("postBond", args);
    if (ltLoader->gotLens())
    {
        SCTimer t("moveFromPlacePosToPickLensPos", bondingTimeLog());

        checkLensIndex();
        PrOffset lensPrOffset = ltLoader->getLastLensOffset();
        QPointF pickLensPos(bhConfig->bhToLTCameraOffsetX() + lensPrOffset.X,
                            bhConfig->bhToLTCameraOffsetY() + lensPrOffset.Y);
        if (bhConfig->useLtSideTwoDimCaliData())
        {
            pickLensPos = ltSideTdc->nnA2B(pickLensPos);
        }

        auto *standbyPos = bhXyz->getPos<XYZModulePos>(XYZPosName::StandbyPos);
        bhXyz->zAxis()->waitGreaterThanPos(qMin(bhConfig->stSafetyHeightAfterPlaceLens(), standbyPos->zPos()));
        bhXyz->xAxis()->absMove(pickLensPos.x(), false);
        zyPlanner->moveTo(standbyPos->zPos(), pickLensPos.y(), bhConfig->safetyPosNearST()->yPos(),
                          bhConfig->safetyPosNearST()->zPos(), TwoAxisPlanner::GreaterThan, false, false);

        QElapsedTimer timer;
        timer.start();
        while (true)
        {
            if (hasAbortEvent())
            {
                return;
            }
            if (timer.elapsed() > 3000)
            {
                qCWarning(logCate()) << "Waiting bond head y cross standby pos...";
                timer.restart();
            }
            double yPos = bhXyz->yAxis()->getFeedbackPos();
            if (yPos < standbyPos->yPos())
            {
                break;
            }
            if (abs(yPos - pickLensPos.y()) < 0.1)
            {
                break;
            }
            QThread::msleep(20);
        }
        bwh->pushEvent(bwhPostBondEvent);
        pushGetSensorEventToBWH();

        SingleAxisPos *zPickLensPos = bhZ->getPos<SingleAxisPos>(SAxisPosName::LTPickPlacePos);
        yzPlanner->moveTo(pickLensPos.y(), zPickLensPos->pos(), bhConfig->safetyPosNearLT()->zPos(),
                          bhConfig->safetyPosNearLT()->yPos(), TwoAxisPlanner::LessThan, false);
        bhXyz->xAxis()->waitArrivedPos();

        pushEvent(WorkerEvent("pickLens"));
    }
    else
    {
        SCTimer t("moveFromPlacePosToStandbyPos", bondingTimeLog());

        moveFromStToStandbyPosImpl();
        bwh->pushEvent(bwhPostBondEvent);
        pushGetSensorEventToBWH();
        pushEvent(WorkerEvent("waitLens"));
        pushEvent(WorkerEvent("directMoveToPickLensPos"));
        pushEvent(WorkerEvent("pickLens"));
    }
}

void BonderHead::bwhStopped()
{
    isBwhStopped = true;
}

void BonderHead::readLTCaliGlassPrOffset()
{
    checkIsInit();
    PrOffset prOffset = getLTCaliGlassPrOffset();
    bhConfig->setLtCaliGlassPrOffsetX(round(prOffset.X * 10000) / 10000);
    bhConfig->setLtCaliGlassPrOffsetY(round(prOffset.Y * 10000) / 10000);
}

void BonderHead::estimateBHToLTCameraOffset()
{
    checkIsInit();
    auto pickLTCaliGlassPos = bhXyz->getPos<XYZModulePos>(XYZPosName::LTPickPlaceCaliGlassPos);
    bhConfig->setBhToLTCameraOffsetX(pickLTCaliGlassPos->xPos() - bhConfig->ltCaliGlassPrOffsetX());
    bhConfig->setBhToLTCameraOffsetY(pickLTCaliGlassPos->yPos() - bhConfig->ltCaliGlassPrOffsetY());
}

void BonderHead::ltCaliGlassStaticPrDeviationTest(int times)
{
    checkIsInit();

    if (!UIOperation::getIns()->okCancelConfirm(tr("Please place calibration glass at the view of lens tray camera!")))
    {
        return;
    }

    if (times < 2)
    {
        qCWarning(logCate()) << tr("Times was too small! Auto change to 2!");
        times = 2;
    }
    QList<double> xOffsets;
    QList<double> yOffset;
    QList<double> thetaOffsets;
    for (int i = 0; i < times; i++)
    {
        PrOffset prOffset = getLTCaliGlassPrOffset();
        xOffsets.append(prOffset.X);
        yOffset.append(prOffset.Y);
        thetaOffsets.append(prOffset.Theta);
    }

    showDeviationTestResult(tr("Lens tray calibration glass static pr deviation test succesful!"),
                            "ltCaliGlassStaticPrDeviationTest", xOffsets, yOffset, thetaOffsets);
}

void BonderHead::ltCaliGlassStaticPickPlaceDeviationTest(int times, int delayAfterMoveToPickPos)
{
    checkIsInit();

    if (!UIOperation::getIns()->okCancelConfirm(tr("Please place calibration glass at the view of lens tray camera!")))
    {
        return;
    }

    if (times < 2)
    {
        qCWarning(logCate()) << tr("Times was too small! Auto change to 2!");
        times = 2;
    }

    QList<double> xOffsets;
    QList<double> yOffset;
    QList<double> thetaOffsets;

    moveToStandbyPos(true);

    for (int i = 0; i < times + 1; i++)
    {
        PrOffset prOffset = getLTCaliGlassPrOffset();
        xOffsets.append(prOffset.X);
        yOffset.append(prOffset.Y);
        thetaOffsets.append(prOffset.Theta);
        if (i < times)
        {
            bhXyz->moveXY(bhConfig->bhToLTCameraOffsetX() + prOffset.X, bhConfig->bhToLTCameraOffsetY() + prOffset.Y);
            bhXyz->moveZToPos(XYZPosName::LTPickPlaceCaliGlassPos);
            if (delayAfterMoveToPickPos > 0)
            {
                QThread::msleep(delayAfterMoveToPickPos);
            }
            pickDut(false);
            placeDut(true);
            bhXyz->moveXYToPos(XYZPosName::StandbyPos);
        }
    }

    showDeviationTestResult(tr("Lens tray calibration glass static pick and place deviation test succesful!"),
                            "ltCaliGlassStaticPickPlaceDeviationTest", xOffsets, yOffset, thetaOffsets);
}

void BonderHead::calibrateBHToLTCameraOffset()
{
    checkIsInit();

    if (!UIOperation::getIns()->okCancelConfirm(tr("Please place calibration glass at the view of lens tray camera!")))
    {
        return;
    }
    moveToStandbyPos(true);
    moveToPickLTCaliGlass();

    bhXyz->moveTo(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    QThread::msleep(400);
    PrOffset prOffset = getCaliGlassUplookPrOffset();
    QPointF corToUplookCameraOffset = getCorToUplookCameraOffset();
    QPointF pickShift = corToUplookCameraOffset - QPointF(prOffset.X, prOffset.Y);

    double newBhToLTCameraOffsetX = bhConfig->bhToLTCameraOffsetX() - pickShift.x();
    double newBhToLTCameraOffsetY = bhConfig->bhToLTCameraOffsetY() - pickShift.y();
    QString bhToLTCameraMsg
        = tr("Calibrate bonder head to lens tray camera offset successful!\r\nOffset x: %1, Offset y: "
             "%2\r\nNew bonder head to camera offset x: %3, y: %4\r\nWould you like to use the new offset ?")
              .arg(pickShift.x())
              .arg(pickShift.y())
              .arg(newBhToLTCameraOffsetX)
              .arg(newBhToLTCameraOffsetY);
    if (UIOperation::getIns()->yesNoConfirm(bhToLTCameraMsg))
    {
        bhConfig->setBhToLTCameraOffsetX(newBhToLTCameraOffsetX);
        bhConfig->setBhToLTCameraOffsetY(newBhToLTCameraOffsetY);
    }
}

void BonderHead::moveToPickLTCaliGlass()
{
    checkIsInit();
    PrOffset prOffset = getLTCaliGlassPrOffset();
    bhTheta->moveTo(SAxisPosName::StandbyPos, false);
    bhXyz->moveZToPos(XYZPosName::StandbyPos);
    bhXyz->moveXY(bhConfig->bhToLTCameraOffsetX() + prOffset.X, bhConfig->bhToLTCameraOffsetY() + prOffset.Y);
    bhXyz->moveZToPos(XYZPosName::LTPickPlaceCaliGlassPos);
    bhTheta->waitArrivedPos(SAxisPosName::StandbyPos);
    pickDut(true);
    bhXyz->moveTo(XYZPosName::UplookPos, XYZModulePos::Z_XY);
}

void BonderHead::moveToCalibrateCor()
{
    if (!UIOperation::getIns()->okCancelConfirm(tr("Please make sure bond head has picked calibration glass!")))
    {
        return;
    }

    bhXyz->moveTo(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    QThread::msleep(400);
    QPointF corToUplookCameraOffset = getCorToUplookCameraOffset();

    double currentBHXPos = bhXyz->xAxis()->getFeedbackPos(-1);
    double currentBHYPos = bhXyz->yAxis()->getFeedbackPos(-1);

    double bhCorToUplookCameraOffsetX = corToUplookCameraOffset.x() - currentBHXPos;
    double bhCorToUplookCameraOffsetY = corToUplookCameraOffset.y() - currentBHYPos;
    QString bhCorToUplookCameraMsg
        = tr("Calibrate bonder head COR to up look camera offset successful!\r\nBhCorToUplookCameraOffsetX: %1, "
             "Y: %2\r\nWould you like to use this offset?")
              .arg(bhCorToUplookCameraOffsetX)
              .arg(bhCorToUplookCameraOffsetY);
    if (UIOperation::getIns()->yesNoConfirm(bhCorToUplookCameraMsg))
    {
        bhConfig->setBhCorToUplookCameraOffsetX(bhCorToUplookCameraOffsetX);
        bhConfig->setBhCorToUplookCameraOffsetY(bhCorToUplookCameraOffsetY);
    }
}

void BonderHead::calibrateLTCameraToUplookCameraOffset()
{
    checkIsInit();

    moveToStandbyPos(true);
    if (bhVacuum->get())
    {
        if (!UIOperation::getIns()->okCancelConfirm(tr("Make sure bond head has picked calibration glass!")))
        {
            return;
        }
    }
    else
    {
        if (!UIOperation::getIns()->okCancelConfirm(
                tr("Please place calibration glass at the view of lens tray camera!")))
        {
            return;
        }
        moveToPickLTCaliGlass();
    }

    bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    QThread::msleep(200);
    PrOffset uplookOffset = getCaliGlassUplookPrOffset();

    bhXyz->moveXY(bhConfig->bhToLTCameraOffsetX() - bhConfig->ltCameraCenterXShift(), bhConfig->bhToLTCameraOffsetY());
    bhXyz->moveZToPos(XYZPosName::LTPickPlaceCaliGlassPos);
    placeDut(true);
    double placeDutBhXPos = bhXyz->xAxis()->getFeedbackPos(-1);
    double placeDutBhYPos = bhXyz->yAxis()->getFeedbackPos(-1);
    bhXyz->moveXYToPos(XYZPosName::StandbyPos);
    PrOffset downlookOffset = getLTCaliGlassPrOffset();

    auto uplookPos = bhXyz->getPos<XYZModulePos>(XYZPosName::UplookPos);
    double lTCameraToUplookCameraOffsetX = placeDutBhXPos - uplookPos->xPos() - downlookOffset.X + uplookOffset.X;
    double lTCameraToUplookCameraOffsetY = placeDutBhYPos - uplookPos->yPos() - downlookOffset.Y + uplookOffset.Y;
    double lTCameraToUplookCameraOffsetTheta = downlookOffset.Theta + uplookOffset.Theta;

    QString msg = tr("Calibrate lens tray camera to uplook camera offset successful!\r\nOffset x: %1, y: %2, "
                     "theta: %3\r\nWould you like to use this offset?")
                      .arg(lTCameraToUplookCameraOffsetX)
                      .arg(lTCameraToUplookCameraOffsetY)
                      .arg(lTCameraToUplookCameraOffsetTheta);

    if (UIOperation::getIns()->yesNoConfirm(msg))
    {
        bhConfig->ltCameraToUplookCameraOffset()->setX(lTCameraToUplookCameraOffsetX);
        bhConfig->ltCameraToUplookCameraOffset()->setY(lTCameraToUplookCameraOffsetY);
        bhConfig->ltCameraToUplookCameraOffset()->setTheta(lTCameraToUplookCameraOffsetTheta);
    }
}

void BonderHead::doLensTraySideTwoDimensionCalibration()
{
    checkIsInit();
    if (!UIOperation::getIns()->okCancelConfirm(
            tr("Please place calibration glass at the view of lens tray downlook camera, and make sure you have "
               "calibrated lens tray camera to uplook camera offset!")))
    {
        return;
    }

    doTwoDimensionCalibration(false, 0);
    ltSideTdc->save(ltSideTdcFileName());
    qDebug(logCate()) << tr("Lens tray side two dimension calibration successful!");
    ltSideTdc->printSelf();

    UIOperation::getIns()->showTip(tr("Do lens tray side two dimension calibration successful!"));
}

void BonderHead::placeCaliGlassToLTCameraCenter()
{
    checkIsInit();
    bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    QThread::msleep(100);
    PrOffset uplookOffset = getCaliGlassUplookPrOffset();
    moveToPlaceCaliGlassToLTCameraPos(uplookOffset, bhConfig->useCorFromCalibration(),
                                      bhConfig->ltCameraCenterXShift() * -1);
    placeDut(true);
    bhXyz->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
}

void BonderHead::readSTCaliGlassPrOffset()
{
    checkIsInit();
    PrOffset prOffset = getSTCaliGlassPrOffset();
    double currentOpticYPos = bwhOpticXY->yAxis()->getFeedbackPos(-1);
    bhConfig->setStCaliGlassPrOffsetX(prOffset.X);
    bhConfig->setStCaliGlassPrOffsetY(prOffset.Y + currentOpticYPos);
}

void BonderHead::stCaliGlassStaticPrDeviationTest(int times)
{
    checkIsInit();

    if (!UIOperation::getIns()->okCancelConfirm(
            tr("Please place calibration glass at the view of bond work holder camera!")))
    {
        return;
    }

    if (times < 2)
    {
        qCWarning(logCate()) << tr("Times was too small! Auto change to 2!");
        times = 2;
    }
    QList<double> xOffsets;
    QList<double> yOffset;
    QList<double> thetaOffsets;
    for (int i = 0; i < times; i++)
    {
        PrOffset prOffset = getSTCaliGlassPrOffset();
        xOffsets.append(prOffset.X);
        yOffset.append(prOffset.Y);
        thetaOffsets.append(prOffset.Theta);
    }

    showDeviationTestResult(tr("Sensor tray calibration glass static pr deviation test succesful!"),
                            "stCaliGlassStaticPrDeviationTest", xOffsets, yOffset, thetaOffsets);
}

void BonderHead::stCaliGlassStaticPickPlaceDeviationTest(int times, int delayAfterMoveToPickPos)
{
    checkIsInit();

    if (!UIOperation::getIns()->okCancelConfirm(
            tr("Please place calibration glass at the view of bond work holder camera!")))
    {
        return;
    }

    if (times < 2)
    {
        qCWarning(logCate()) << tr("Times was too small! Auto change to 2!");
        times = 2;
    }

    QList<double> xOffsets;
    QList<double> yOffset;
    QList<double> thetaOffsets;

    bwhOpticZ->moveTo(SAxisPosName::CaliGlassPrPos, false);
    moveToStandbyPos(true);
    bwhOpticZ->waitArrivedPos(SAxisPosName::CaliGlassPrPos);

    double currentOpticYPos = bwhOpticXY->yAxis()->getFeedbackPos(-1);
    for (int i = 0; i < times + 1; i++)
    {
        PrOffset prOffset = getSTCaliGlassPrOffset();
        xOffsets.append(prOffset.X);
        yOffset.append(prOffset.Y);
        thetaOffsets.append(prOffset.Theta);
        if (i < times)
        {
            bhXyz->moveXY(bhConfig->bhToSTCameraOffsetX() + prOffset.X,
                          bhConfig->bhToSTCameraOffsetY() + prOffset.Y + currentOpticYPos);
            bhXyz->moveZToPos(XYZPosName::STPickPlaceCaliGlassPos);
            if (delayAfterMoveToPickPos > 0)
            {
                QThread::msleep(delayAfterMoveToPickPos);
            }
            pickDut(false);
            placeDut(true);
            bhXyz->moveXYToPos(XYZPosName::StandbyPos);
        }
    }

    showDeviationTestResult(tr("Sensor tray calibration glass static pick and place deviation test succesful!"),
                            "stCaliGlassStaticPickPlaceDeviationTest", xOffsets, yOffset, thetaOffsets);
}

void BonderHead::stCaliGlassPickPlaceDeviationTest(int times)
{
    checkIsInit();

    if (!UIOperation::getIns()->okCancelConfirm(
            tr("Please place calibration glass at the view of bond work holder camera!")))
    {
        return;
    }

    if (times < 2)
    {
        qCWarning(logCate()) << tr("Times was too small! Auto change to 2!");
        times = 2;
    }

    bwhOpticZ->moveTo(SAxisPosName::CaliGlassPrPos);

    QList<double> pickXOffsets;
    QList<double> pickYOffsets;
    QList<double> pickThetaOffsets;

    QList<double> placeXOffsets;
    QList<double> placeYOffset;
    QList<double> placeThetaOffsets;

    for (int i = 0; i < times; ++i)
    {
        moveToPickSTCaliGlass();
        PrOffset uplookOffset = getCaliGlassUplookPrOffset();
        pickXOffsets.append(uplookOffset.X);
        pickYOffsets.append(uplookOffset.Y);
        pickThetaOffsets.append(uplookOffset.Theta);

        placeCaliGlassToSTCameraCenter();
        PrOffset caliGlassOffset = getSTCaliGlassPrOffset();
        placeXOffsets.append(caliGlassOffset.X);
        placeYOffset.append(caliGlassOffset.Y);
        placeThetaOffsets.append(caliGlassOffset.Theta);
    }

    showDeviationTestResult(tr("Sensor tray caliglass pick deviation test successful!"), "stCaliGlassPickDeviationTest",
                            pickXOffsets, pickYOffsets, pickThetaOffsets);
    showDeviationTestResult(tr("Sensor tray caliglass place deviation test successful!"),
                            "stCaliGlassPlaceDeviationTest", placeXOffsets, placeYOffset, placeThetaOffsets);
}

void BonderHead::estimateBHToSTCameraOffset()
{
    checkIsInit();

    auto pickSTCaliGlassPos = bhXyz->getPos<XYZModulePos>(XYZPosName::STPickPlaceCaliGlassPos);
    bhConfig->setBhToSTCameraOffsetX(pickSTCaliGlassPos->xPos() - bhConfig->stCaliGlassPrOffsetX());
    bhConfig->setBhToSTCameraOffsetY(pickSTCaliGlassPos->yPos() - bhConfig->stCaliGlassPrOffsetY());
}

void BonderHead::calibrateBHToSTCameraOffset()
{
    checkIsInit();

    if (!UIOperation::getIns()->okCancelConfirm(
            tr("Please place calibration glass at the view of bond work holder camera!")))
    {
        return;
    }

    moveToStandbyPos(true);
    moveToPickSTCaliGlass();

    bhXyz->moveTo(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    QThread::msleep(400);
    PrOffset prOffset = getCaliGlassUplookPrOffset();
    QPointF corToUplookCameraOffset = getCorToUplookCameraOffset();
    QPointF pickShift = corToUplookCameraOffset - QPointF(prOffset.X, prOffset.Y);

    double newBhToSTCameraOffsetX = bhConfig->bhToSTCameraOffsetX() - pickShift.x();
    double newBhToSTCameraOffsetY = bhConfig->bhToSTCameraOffsetY() - pickShift.y();

    QString msg = tr("Calibrate bonder head to sensor tray camera offset successful!\r\nOffset x: %1, Offset y: "
                     "%2\r\nNew bonder head to camera offset x: %3, y: %4\r\nWould you like to use the new offset ?")
                      .arg(pickShift.x())
                      .arg(pickShift.y())
                      .arg(newBhToSTCameraOffsetX)
                      .arg(newBhToSTCameraOffsetY);
    if (UIOperation::getIns()->yesNoConfirm(msg))
    {
        bhConfig->setBhToSTCameraOffsetX(newBhToSTCameraOffsetX);
        bhConfig->setBhToSTCameraOffsetY(newBhToSTCameraOffsetY);
    }
}

void BonderHead::moveToPickSTCaliGlass()
{
    checkIsInit();

    moveTwoZAxisToStandbyPos();
    moveToPickSTCaliGlassPos();
    pickDut(true);
    bhXyz->moveTo(XYZPosName::UplookPos, XYZModulePos::Z_XY);
}

void BonderHead::calibrateSTCameraToUplookCameraOffset()
{
    checkIsInit();

    if (!UIOperation::getIns()->okCancelConfirm(
            tr("Please place calibration glass at the view of bond work holder camera!")))
    {
        return;
    }

    moveToPickSTCaliGlass();
    bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    QThread::msleep(200);
    PrOffset uplookOffset = getCaliGlassUplookPrOffset();

    double currentPoticYPos = bwhOpticXY->yAxis()->getFeedbackPos(-1);

    bhXyz->moveXY(bhConfig->bhToSTCameraOffsetX() - bhConfig->stCameraCenterXShift(),
                  bhConfig->bhToSTCameraOffsetY() + currentPoticYPos);
    bhXyz->moveZToPos(XYZPosName::STPickPlaceCaliGlassPos);
    placeDut(true);
    double placeDutBhXPos = bhXyz->xAxis()->getFeedbackPos(-1);
    double placeDutBhYPos = bhXyz->yAxis()->getFeedbackPos(-1);
    bhXyz->moveXYToPos(XYZPosName::StandbyPos);
    PrOffset downlookOffset = getSTCaliGlassPrOffset();

    auto uplookPos = bhXyz->getPos<XYZModulePos>(XYZPosName::UplookPos);
    double sTCameraToUplookCameraOffsetX = placeDutBhXPos - uplookPos->xPos() - downlookOffset.X + uplookOffset.X;
    double sTCameraToUplookCameraOffsetY
        = placeDutBhYPos - uplookPos->yPos() - currentPoticYPos - downlookOffset.Y + uplookOffset.Y;
    double sTCameraToUplookCameraOffsetTheta = downlookOffset.Theta + uplookOffset.Theta;

    QString msg = tr("Calibrate sensor tray camera to uplook camera offset successful!\r\nOffset x: %1, y: %2, "
                     "theta: %3\r\nWould you like to use this offset?")
                      .arg(sTCameraToUplookCameraOffsetX)
                      .arg(sTCameraToUplookCameraOffsetY)
                      .arg(sTCameraToUplookCameraOffsetTheta);

    if (UIOperation::getIns()->yesNoConfirm(msg))
    {
        bhConfig->stCameraToUplookCameraOffset()->setX(sTCameraToUplookCameraOffsetX);
        bhConfig->stCameraToUplookCameraOffset()->setY(sTCameraToUplookCameraOffsetY);
        bhConfig->stCameraToUplookCameraOffset()->setTheta(sTCameraToUplookCameraOffsetTheta);
    }
}

void BonderHead::placeCaliGlassToSTCameraCenter()
{
    checkIsInit();
    moveTwoZAxisToStandbyPos();
    bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::XYZ);
    QThread::msleep(100);
    PrOffset uplookOffset = getCaliGlassUplookPrOffset();
    moveToPlaceCaliGlassToSTCameraPos(uplookOffset, bhConfig->stCameraToUplookCameraOffset(),
                                      bhConfig->useCorFromCalibration(), bhConfig->stCameraCenterXShift() * -1);
    placeDut(true);
    bhXyz->moveTo(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
}

void BonderHead::resetSTCameraToUplookCameraOffsetOfEachRow()
{
    bhConfig->resetStCameraToUplookCameraOffsetOfEachRow(sensorTrayConfig->rows());
}

void BonderHead::calibrateSTCameraToUplookCameraOffsetOfEachRow()
{
    checkIsInit();
    if (sensorTrayConfig->rows() < 1)
    {
        throw SilicolAbort(tr("Sensor tray rows must greater than 0!"), EX_LOCATION);
    }
    if (!UIOperation::getIns()->okCancelConfirm(
            tr("Please place calibration glass at the view of bond work holder camera!")))
    {
        return;
    }

    while (bhConfig->stCameraToUplookCameraOffsetOfEachRow()->count() > sensorTrayConfig->rows())
    {
        bhConfig->stCameraToUplookCameraOffsetOfEachRow()->remove(
            bhConfig->stCameraToUplookCameraOffsetOfEachRow()->count() - 1);
    }
    while (bhConfig->stCameraToUplookCameraOffsetOfEachRow()->count() < sensorTrayConfig->rows())
    {
        bhConfig->stCameraToUplookCameraOffsetOfEachRow()->add(
            bhConfig->stCameraToUplookCameraOffsetOfEachRow()->count());
    }

    auto uplookPos = bhXyz->getPos<XYZModulePos>(XYZPosName::UplookPos);
    for (int i = 0; i < sensorTrayConfig->rows(); i++)
    {
        moveToPickSTCaliGlass();
        bwhOpticXY->yAxis()->absMove(bwh->getBWHOpticYPrPos(i), false);
        bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::XYZ);
        QThread::msleep(200);
        PrOffset uplookOffset = getCaliGlassUplookPrOffset();
        bwhOpticXY->yAxis()->waitArrivedPos();
        moveToPlaceCaliGlassToSTCameraPos(uplookOffset, bhConfig->stCameraToUplookCameraOffset(), false,
                                          bhConfig->stCameraCenterXShift() * -1);
        placeDut(true);
        double placeDutBhXPos = bhXyz->xAxis()->getFeedbackPos(-1);
        double placeDutBhYPos = bhXyz->yAxis()->getFeedbackPos(-1);
        bhXyz->moveXYToPos(XYZPosName::StandbyPos);
        PrOffset downlookOffset = getSTCaliGlassPrOffset();

        double currentOpticYPos = bwhOpticXY->yAxis()->getFeedbackPos(-1);
        double offsetX = placeDutBhXPos - uplookPos->xPos() - downlookOffset.X + uplookOffset.X;
        double offsetY = placeDutBhYPos - uplookPos->yPos() - currentOpticYPos - downlookOffset.Y + uplookOffset.Y;
        double offsetTheta = downlookOffset.Theta + uplookOffset.Theta;
        auto cameraOffset = bhConfig->stCameraToUplookCameraOffsetOfEachRow()->getConfig<CameraOffset>(i);
        cameraOffset->setX(offsetX);
        cameraOffset->setY(offsetY);
        cameraOffset->setTheta(offsetTheta);
    }
    UIOperation::getIns()->showTip(tr("Calibrate sensor tray camera to uplook camera offset of each row successful!"));
}

void BonderHead::doSensorTraySideTwoDimensionCalibration(int rowIndex, bool calibrateAllRows)
{
    checkIsInit();

    if (sensorTrayConfig->rows() < 1)
    {
        throw SilicolAbort(tr("Sensor tray rows must greater than 0!"), EX_LOCATION);
    }
    if (bhConfig->stCameraToUplookCameraOffsetOfEachRow()->count() != sensorTrayConfig->rows())
    {
        UIOperation::getIns()->showError(
            tr("Please calibrate sensor tray camera to uplook camera offset of each row first!"));
        return;
    }
    if (!calibrateAllRows && (rowIndex < 0 || rowIndex > sensorTrayConfig->rows()))
    {
        UIOperation::getIns()->showError(tr("Row index exceeded limit! [%1, %2]").arg(0).arg(sensorTrayConfig->rows()));
        return;
    }
    if (rowIndex != 0)
    {
        rowIndex--;
    }
    if (!UIOperation::getIns()->okCancelConfirm(
            tr("Please place calibration glass at the view of bond work holder camera, and make sure you have "
               "calibrated sensor tray camera to uplook camera offset of each row!")))
    {
        return;
    }

    for (int i = 0; i < sensorTrayConfig->rows(); i++)
    {
        if (!calibrateAllRows && i != rowIndex)
        {
            continue;
        }
        if (stSideTdcs.count() <= i)
        {
            stSideTdcs.append(new TwoDimensionCalibration());
        }
        doTwoDimensionCalibration(true, i);
        stSideTdcs[i]->save(stSideTdcFileName(i));
        qDebug(logCate()) << tr("Sensor tray side two dimension calibration successful! Row index: %1").arg(i);
        stSideTdcs[i]->printSelf();
    }

    UIOperation::getIns()->showTip(tr("Do sensor tray side two dimension calibration successful!"));
}

void BonderHead::testBondOfEachRow(double maxRandomXYOffsetForPick_mm,
                                   double maxRandomRotationAfterPick_deg,
                                   bool useCorrespondingCalibrationForEachRow,
                                   double targetMaxRandomXOffset_pixel,
                                   double targetMaxRandomYOffset_pixel,
                                   double targetMaxRandomThetaOffset_deg,
                                   int testTimes)
{
    checkIsInit();
    if (sensorTrayConfig->rows() < 1)
    {
        throw SilicolAbort(tr("Sensor tray rows must greater than 0!"), EX_LOCATION);
    }
    if (!UIOperation::getIns()->okCancelConfirm(
            tr("Please place calibration glass at the view of bond work holder camera!")))
    {
        return;
    }
    if (isTestBondOfEachRow)
    {
        qCWarning(logCate()) << tr("testBondOfEachRow is running!");
        return;
    }

    maxRandomXYOffsetForPick_mm = qAbs(maxRandomXYOffsetForPick_mm);
    maxRandomRotationAfterPick_deg = qAbs(maxRandomRotationAfterPick_deg);
    srand(time(0));

    PrOffset prOffset = getSTCaliGlassPrOffset();
    double currentBWHOpticYPos = bwhOpticXY->yAxis()->getFeedbackPos(-1);

    QFile logFile(
        QString("./testBondData/%1.csv").arg(QDateTime::currentDateTime().toString("yyyy_MM_dd_hh_mm_ss_zzz")));
    QDir dir("./testBondData");
    if (!dir.exists())
    {
        dir.mkpath(dir.absolutePath());
    }
    logFile.open(QIODevice::WriteOnly);
    QString title = "rowIndex,xOffsetForPick,yOffsetForPick,thetaOffsetAfterPick,targetOffsetX,targetOffsetY,"
                    "targetOffsetTheta,realPlaceOffsetX,realPlaceOffsetY,"
                    "realPlaceOffsetTheta,postBondXShift_um,postBondYShift_um,postBondThetaShift_deg\r\n";
    logFile.write(title.toUtf8());

    isTestBondOfEachRow = true;

    moveToStandbyPos(true);
    try
    {
        for (int i = 0; i < testTimes; i++)
        {
            for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
            {
                if (!isTestBondOfEachRow)
                {
                    logFile.flush();
                    logFile.close();
                    return;
                }

                double xOffsetForPick = 0;
                double yOffsetForPick = 0;
                double rotationOffsetAfterPick = 0;
                if (!qFuzzyCompare(maxRandomXYOffsetForPick_mm, 0))
                {
                    xOffsetForPick = getRand(-maxRandomXYOffsetForPick_mm, maxRandomXYOffsetForPick_mm);
                    yOffsetForPick = getRand(-maxRandomXYOffsetForPick_mm, maxRandomXYOffsetForPick_mm);
                }
                if (!qFuzzyCompare(maxRandomRotationAfterPick_deg, 0))
                {
                    rotationOffsetAfterPick = getRand(-maxRandomRotationAfterPick_deg, maxRandomRotationAfterPick_deg);
                }
                bwhOpticXY->yAxis()->absMove(bwh->getBWHOpticYPrPos(rowIndex), false);
                bhTheta->moveTo(SAxisPosName::StandbyPos, false);
                bhXyz->moveXY(bhConfig->bhToSTCameraOffsetX() + prOffset.X + xOffsetForPick,
                              bhConfig->bhToSTCameraOffsetY() + prOffset.Y + currentBWHOpticYPos + yOffsetForPick);
                bhXyz->moveZToPos(XYZPosName::STPickPlaceCaliGlassPos);
                bhTheta->waitArrivedPos(SAxisPosName::StandbyPos);
                pickDut(true);

                bhThetaMotor->relMove(rotationOffsetAfterPick, false);
                bhXyz->moveXYToPos(XYZPosName::LTPickPlaceCaliGlassPos);
                bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::Z_XY);
                bhThetaMotor->waitArrivedPos();
                QThread::msleep(100);
                PrOffset uplookOffset = getCaliGlassUplookPrOffset();

                bwhOpticXY->yAxis()->waitArrivedPos();
                CameraOffset *cameraOffset
                    = useCorrespondingCalibrationForEachRow
                          ? bhConfig->stCameraToUplookCameraOffsetOfEachRow()->getConfig<CameraOffset>(rowIndex)
                          : bhConfig->stCameraToUplookCameraOffset();
                TwoDimensionCalibration *tdc = bhConfig->useStSideTwoDimCaliData() ? stSideTdc(rowIndex) : nullptr;

                PRResultStruct givenPrResult;
                givenPrResult.x
                    = qFuzzyCompare(targetMaxRandomXOffset_pixel, 0)
                          ? 0
                          : getRand(-qAbs(targetMaxRandomXOffset_pixel), qAbs(targetMaxRandomXOffset_pixel));
                givenPrResult.y
                    = qFuzzyCompare(targetMaxRandomYOffset_pixel, 0)
                          ? 0
                          : getRand(-qAbs(targetMaxRandomYOffset_pixel), qAbs(targetMaxRandomYOffset_pixel));
                givenPrResult.theta
                    = qFuzzyCompare(targetMaxRandomThetaOffset_deg, 0)
                          ? 0
                          : getRand(-qAbs(targetMaxRandomThetaOffset_deg), qAbs(targetMaxRandomThetaOffset_deg));
                PrOffset givenPrOffset;
                if (!stCaliGlassLocation->result2Offset(givenPrResult, givenPrOffset))
                {
                    throw SilicolAbort(tr("Pixel to micro failed!"));
                }

                moveToPlaceCaliGlassToSTCameraPos(uplookOffset, cameraOffset, bhConfig->useCorFromCalibration(),
                                                  givenPrOffset.X, givenPrOffset.Y, givenPrOffset.Theta, tdc);

                placeDut(true);
                bhXyz->moveXYToPos(XYZPosName::StandbyPos);
                prOffset = getSTCaliGlassPrOffset();
                currentBWHOpticYPos = bwhOpticXY->yAxis()->getFeedbackPos(-1);

                PrOffset postBondShift = (givenPrOffset - prOffset) * 1000;    // mm to um
                bwh->showPostBondShift(postBondShift);

                QString logLine = QString("%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13\r\n")
                                      .arg(rowIndex)
                                      .arg(xOffsetForPick)
                                      .arg(yOffsetForPick)
                                      .arg(rotationOffsetAfterPick)
                                      .arg(givenPrOffset.X)
                                      .arg(givenPrOffset.Y)
                                      .arg(givenPrOffset.Theta)
                                      .arg(prOffset.X)
                                      .arg(prOffset.Y)
                                      .arg(prOffset.Theta)
                                      .arg(postBondShift.X)
                                      .arg(postBondShift.Y)
                                      .arg(postBondShift.Theta);
                logFile.write(logLine.toUtf8());
                logFile.flush();
            }
        }

        logFile.flush();
        logFile.close();
        isTestBondOfEachRow = false;
    }
    catch (SilicoolException &se)
    {
        logFile.flush();
        logFile.close();
        isTestBondOfEachRow = false;
        throw se;
    }
}

void BonderHead::stopTestBondOfEachRow()
{
    isTestBondOfEachRow = false;
}

void BonderHead::manuallyPickLens(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    moveToStandbyPos(true);

    // Warn: The following two lines can not be exchanged the order!
    ltLoader->moveToPrPos(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    lensTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);

    PrOffset prOffset = ltLoader->getLensPrOffset(unitRowIndex * lensTrayConfig->rowsInsideUnit() + rowIndex);
    directMoveToPickLensPosImpl(prOffset);
    pickDutDown();
    softlandUpMiniZWithoutWait();
    bhXyz->moveZToPos(XYZPosName::UplookPos, false);
    checkDutAfterPick();
    pickedLensIndex = QPoint(unitRowIndex * lensTrayConfig->rowsInsideUnit() + rowIndex,
                             unitColumnIndex * lensTrayConfig->columnsInsideUnit() + columnIndex);
    moveFromLtToUplookPosImpl();
}

void BonderHead::moveToPerformUplookPr()
{
    checkIsInit();
    bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    getLensUplookOffset();
}

void BonderHead::manuallyPlaceBackLens(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    moveToStandbyPos(true);

    // Warn: The following two lines can not be exchanged the order!
    ltLoader->moveToPrPos(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    lensTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);

    PrOffset ltVacancyPrOffset
        = ltLoader->getLTVacancyPrOffset(unitRowIndex * lensTrayConfig->rowsInsideUnit() + rowIndex);

    bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    PrOffset uplookOffset = getLensUplookRectOffset();

    double placeZHeight = bhZ->getPos<SingleAxisPos>(SAxisPosName::LTPickPlacePos)->pos();
    TwoDimensionCalibration *tdc = bhConfig->useLtSideTwoDimCaliData() ? ltSideTdc : nullptr;
    if (bhConfig->useCorFromCalibration())
    {
        moveToPlaceDutToCameraPos(uplookOffset, bhConfig->ltCameraToUplookCameraOffset(), placeZHeight, true,
                                  ltVacancyPrOffset.X, ltVacancyPrOffset.Y, ltVacancyPrOffset.Theta, tdc);
    }
    else
    {
        double thetaAngle
            = ltVacancyPrOffset.Theta - (bhConfig->ltCameraToUplookCameraOffset()->theta() - uplookOffset.Theta);
        bhThetaMotor->relMove(thetaAngle, true, 0.001);
        PrOffset offsetAfterRotation = getLensUplookRectOffset();
        moveToPlaceDutToCameraPos(offsetAfterRotation, bhConfig->ltCameraToUplookCameraOffset(), placeZHeight, false,
                                  ltVacancyPrOffset.X, ltVacancyPrOffset.Y, ltVacancyPrOffset.Theta, tdc);
    }

    placeDut(true);
    bhXyz->moveXYToPos(XYZPosName::StandbyPos);
}

void BonderHead::manuallyBond(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex, bool doPostBond)
{
    checkIsInit();

    moveTwoZAxisToStandbyPos();

    // Warn: The following two lines can not be exchanged order
    bwh->moveToPrPos(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    sensorTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);

    int totalRowIndex = unitRowIndex * sensorTrayConfig->rowsInsideUnit() + rowIndex;
    int totalColIndex = unitColumnIndex * sensorTrayConfig->columnsInsideUnit() + columnIndex;
    PrOffset sensorPrOffset = bwh->getSensorPrOffset();
    if (doPostBond)
    {
        bwhOpticZ->moveTo(SAxisPosName::PostBondPrPos, false);
    }

    bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    auto cameraToUplookCameraOffset
        = bhConfig->stCameraToUplookCameraOffsetOfEachRow()->getConfig<CameraOffset>(totalRowIndex);
    if (!bhConfig->useCorFromCalibration())
    {
        PrOffset lensUplookRectOffset = getLensUplookRectOffset();
        double thetaAngle = sensorPrOffset.Theta - (cameraToUplookCameraOffset->theta() - lensUplookRectOffset.Theta);
        bhThetaMotor->relMove(thetaAngle, true, 0.001);
    }
    PrOffset uplookOffset = getLensUplookOffset();
    double targetYOffset = sensorPrOffset.Y + bwhOpticXY->yAxis()->getFeedbackPos(-1);
    TwoDimensionCalibration *tdc = bhConfig->useStSideTwoDimCaliData() ? stSideTdc(totalRowIndex) : nullptr;
    fastMoveToStCameraPlacePos(uplookOffset, cameraToUplookCameraOffset, bhConfig->useCorFromCalibration(),
                               sensorPrOffset.X, targetYOffset, sensorPrOffset.Theta, tdc);
    placeDutDown();
    softlandUpMiniZWithoutWait();
    bhXyz->moveZToPos(XYZPosName::StandbyPos, false);

    moveFromStToStandbyPosImpl();

    if (doPostBond)
    {
        bwhOpticZ->waitArrivedPos(SAxisPosName::PostBondPrPos);
        PrOffset postBondOffset = bwh->getPostBondOffset();
        PrOffset shift = (postBondOffset - sensorPrOffset) * 1000;    // mm to um
        bwh->correctPostBondShift(shift);
        bwh->showPostBondShift(shift);
        auto bwhConfig = bwh->getBwhConfig();
        bool postBondResult
            = qAbs(shift.X) < qAbs(bwhConfig->postBondLimitX()) && qAbs(shift.Y) < qAbs(bwhConfig->postBondLimitY());
        bwh->savePostBondData(QPoint(totalRowIndex, totalColIndex), pickedLensIndex, sensorPrOffset, postBondOffset,
                              true, postBondResult);
    }
}

void BonderHead::doPick()
{
    pickDut(true);
}

void BonderHead::doPlace()
{
    placeDut(true);
}

void BonderHead::readSafetyPosNearLt()
{
    try
    {
        checkIsInit();
        double yPos = bhXyz->yAxis()->getFeedbackPos();
        double zPos = bhXyz->zAxis()->getFeedbackPos();

        bhConfig->safetyPosNearLT()->set(yPos, zPos);
    }
    catch (SilicoolException &se)
    {
        qCCritical(logCate()) << se.what();
    }
}

void BonderHead::readSafetyPosNearSt()
{
    try
    {
        checkIsInit();
        double yPos = bhXyz->yAxis()->getFeedbackPos();
        double zPos = bhXyz->zAxis()->getFeedbackPos();

        bhConfig->safetyPosNearST()->set(yPos, zPos);
    }
    catch (SilicoolException &se)
    {
        qCCritical(logCate()) << se.what();
    }
}

void BonderHead::readLtSafetyHeightAfterPickLens()
{
    try
    {
        checkIsInit();
        bhConfig->setLtSafetyHeightAfterPickLens(bhXyz->zAxis()->getFeedbackPos());
    }
    catch (SilicoolException &se)
    {
        qCCritical(logCate()) << se.what();
    }
}

void BonderHead::readStSafetyHeightAfterPlaceLens()
{
    try
    {
        checkIsInit();
        bhConfig->setStSafetyHeightAfterPlaceLens(bhXyz->zAxis()->getFeedbackPos());
    }
    catch (SilicoolException &se)
    {
        qCCritical(logCate()) << se.what();
    }
}

void BonderHead::ltCaliGlassCustomCaliFunc(double xMove,
                                           double yMove,
                                           QVector<QPointF> &pixelPoints,
                                           QVector<QPointF> &motorPoints)
{
    checkIsInit();

    auto calibration = VisionManager::getIns().getCalibration(CaliName::LTCaliGlassCalibration);
    caliGlassCustomCaliFunc(calibration, XYZPosName::LTPickPlaceCaliGlassPos, xMove, yMove, pixelPoints, motorPoints,
                            true);
}

void BonderHead::ltCaliGlassPerformPrResultFunc(PrOffset prOffset)
{
    checkIsInit();
    moveTwoZAxisToStandbyPos();
    bhXyz->moveXyToPosWithOffset(XYZPosName::LTPickPlaceCaliGlassPos, prOffset.X - bhConfig->ltCaliGlassPrOffsetX(),
                                 prOffset.Y - bhConfig->ltCaliGlassPrOffsetY());
    bhXyz->moveZToPos(XYZPosName::LTPickPlaceCaliGlassPos);
    pickDut(false);
    bhXyz->relMove(-(prOffset.X + bhConfig->ltCameraCenterXShift()), -prOffset.Y);
    placeDut(false);
    bhXyz->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
}

void BonderHead::stCaliGlassCustomCaliFunc(double xMove,
                                           double yMove,
                                           QVector<QPointF> &pixelPoints,
                                           QVector<QPointF> &motorPoints)
{
    checkIsInit();

    auto calibration = VisionManager::getIns().getCalibration(CaliName::STCaliGlassCalibration);
    caliGlassCustomCaliFunc(calibration, XYZPosName::STPickPlaceCaliGlassPos, xMove, yMove, pixelPoints, motorPoints,
                            false);
}

void BonderHead::stCaliGlassPerformPrResultFunc(PrOffset prOffset)
{
    checkIsInit();
    moveTwoZAxisToStandbyPos();
    double currentOpticYPos = bwhOpticXY->yAxis()->getFeedbackPos(-1);
    bhXyz->moveXyToPosWithOffset(XYZPosName::STPickPlaceCaliGlassPos, prOffset.X - bhConfig->stCaliGlassPrOffsetX(),
                                 prOffset.Y - bhConfig->stCaliGlassPrOffsetY() + currentOpticYPos);
    bhXyz->moveZToPos(XYZPosName::STPickPlaceCaliGlassPos);
    pickDut(false);
    bhXyz->relMove(-(prOffset.X + bhConfig->stCameraCenterXShift()), -prOffset.Y);
    placeDut(true);
    bhXyz->moveXYToPos(XYZPosName::StandbyPos);
}

void BonderHead::caliGlassCustomCaliFunc(Calibration *calibration,
                                         QString pickCaliGlassPos,
                                         double xMove,
                                         double yMove,
                                         QVector<QPointF> &pixelPoints,
                                         QVector<QPointF> &motorPoints,
                                         bool isLt)
{

    double pixel_x, pixel_y;
    double xPos = 0, yPos = 0;
    int delayAfterMoveXy = 500;
    moveTwoZAxisToStandbyPos();
    for (int i = 1; i <= 5; i++)
    {
        if (i == 1)
        {
            if (isLt && !qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
            {
                ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift() * -1);
            }

            bhXyz->moveToPos(pickCaliGlassPos, XYZModulePos::XY_Z);
            QThread::msleep(delayAfterMoveXy);
            pickDut(false);
            bhXyz->xAxis()->relMove(xMove);
            QThread::msleep(delayAfterMoveXy);
            placeDut(true);

            if (isLt && !qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
            {
                ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift());
            }

            xPos = bhXyz->xAxis()->getFeedbackPos();
            yPos = bhXyz->yAxis()->getFeedbackPos();
            bhXyz->moveXYToPos(XYZPosName::StandbyPos);
        }
        else if (i == 2)
        {
            if (isLt && !qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
            {
                ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift() * -1);
            }

            bhXyz->moveXyToPosWithOffset(pickCaliGlassPos, xMove, 0);
            bhXyz->moveZToPos(pickCaliGlassPos);
            pickDut(false);
            bhXyz->xAxis()->relMove(xMove * -2);
            QThread::msleep(delayAfterMoveXy);
            placeDut(true);

            if (isLt && !qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
            {
                ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift());
            }

            xPos = bhXyz->xAxis()->getFeedbackPos();
            yPos = bhXyz->yAxis()->getFeedbackPos();
            bhXyz->moveXYToPos(XYZPosName::StandbyPos);
        }
        else if (i == 3)
        {
            if (isLt && !qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
            {
                ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift() * -1);
            }

            bhXyz->moveXyToPosWithOffset(pickCaliGlassPos, -xMove, 0);
            bhXyz->moveZToPos(pickCaliGlassPos);
            pickDut(false);
            bhXyz->relMove(xMove, yMove);
            QThread::msleep(delayAfterMoveXy);
            placeDut(true);

            if (isLt && !qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
            {
                ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift());
            }

            xPos = bhXyz->xAxis()->getFeedbackPos();
            yPos = bhXyz->yAxis()->getFeedbackPos();
            bhXyz->moveXYToPos(XYZPosName::StandbyPos);
        }
        else if (i == 4)
        {
            if (isLt && !qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
            {
                ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift() * -1);
            }

            bhXyz->moveXyToPosWithOffset(pickCaliGlassPos, 0, yMove);
            bhXyz->moveZToPos(pickCaliGlassPos);
            pickDut(false);
            bhXyz->yAxis()->relMove(yMove * -2);
            QThread::msleep(delayAfterMoveXy);
            placeDut(true);

            if (isLt && !qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
            {
                ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift());
            }

            xPos = bhXyz->xAxis()->getFeedbackPos();
            yPos = bhXyz->yAxis()->getFeedbackPos();
            bhXyz->moveXYToPos(XYZPosName::StandbyPos);
        }
        else if (i == 5)
        {
            if (isLt && !qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
            {
                ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift() * -1);
            }

            bhXyz->moveXyToPosWithOffset(pickCaliGlassPos, 0, -yMove);
            bhXyz->moveZToPos(pickCaliGlassPos);
            pickDut(false);
            bhXyz->yAxis()->relMove(yMove);
            QThread::msleep(delayAfterMoveXy);
            placeDut(true);

            bhXyz->moveXYToPos(XYZPosName::StandbyPos);
        }

        if (i != 5)
        {
            calibration->GetPixelPoint(pixel_x, pixel_y);
            qCInfo(visionCate()) << calibration->calibrationName() << "Mech x, y: " << xPos << yPos;
            pixelPoints.append(QPointF(pixel_x, pixel_y));
            motorPoints.append(QPointF(xPos, yPos));
        }
    }
}

void BonderHead::moveToStandbyPos(bool moveBhZFirst)
{
    bhMiniZ->moveTo(SAxisPosName::StandbyPos, false);
    if (moveBhZFirst)
    {
        bhXyz->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
    }
    else
    {
        bhXyz->moveToPos(XYZPosName::StandbyPos, XYZModulePos::XYZ);
    }
    bhMiniZ->waitArrivedPos(SAxisPosName::StandbyPos);
}

void BonderHead::moveTwoZAxisToStandbyPos()
{
    bhMiniZ->moveTo(SAxisPosName::StandbyPos, false);
    bhXyz->moveZToPos(XYZPosName::StandbyPos);
    bhMiniZ->waitArrivedPos(SAxisPosName::StandbyPos);
}

void BonderHead::pickDut(bool upBondHeadZ)
{
    pickDutDown();
    if (upBondHeadZ)
    {
        softlandUpMiniZWithoutWait();
        bhXyz->moveZToPos(XYZPosName::StandbyPos);
    }
    else
    {
        if (bhMiniZMotor->isSoftlandingDown())
        {
            bhMiniZMotor->softLandUp();
        }
        else
        {
            bhMiniZ->moveTo(SAxisPosName::StandbyPos);
        }
    }
    checkDutAfterPick();
}

void BonderHead::placeDut(bool upBondHeadZ)
{
    placeDutDown();
    if (upBondHeadZ)
    {
        softlandUpMiniZWithoutWait();
        bhXyz->moveZToPos(XYZPosName::StandbyPos);
    }
    else
    {
        if (bhMiniZMotor->isSoftlandingDown())
        {
            bhMiniZMotor->softLandUp();
        }
        else
        {
            bhMiniZ->moveTo(SAxisPosName::StandbyPos);
        }
    }
}

void BonderHead::pickDutDown()
{
    if (pickPlaceConfig->delayBeforePick() > 0)
    {
        QThread::msleep(pickPlaceConfig->delayBeforePick());
    }
    try
    {
        if (pickPlaceConfig->delayAfterSoftlandContact() > 0)
        {
            pickDutDownAction();
            QThread::msleep(pickPlaceConfig->delayAfterSoftlandContact());
            bhVacuum->set(true, false);
        }
        else
        {
            softLandingPickDownCmd->reset();
            thdPool->start(softLandingPickDownCmd);
            QElapsedTimer timer;
            timer.start();
            bool hasOpenVacuum = false;
            while (!softLandingPickDownCmd->isFinished())
            {
                if (pickPlaceConfig->openVacuumDelayAfterStartSoftlanding() > 0
                    && timer.elapsed() >= pickPlaceConfig->openVacuumDelayAfterStartSoftlanding())
                {
                    if (!hasOpenVacuum)
                    {
                        bhVacuum->set(true, false);
                        hasOpenVacuum = true;
                    }
                }
                QThread::msleep(1);
            }
            if (!softLandingPickDownCmd->errMsg().isEmpty())
            {
                throw SilicoolException(softLandingPickDownCmd->errMsg());
            }
            if (!hasOpenVacuum)
            {
                bhVacuum->set(true, false);
            }
        }

        if (!bhVacuum->waitUntilTimeout(true))
        {
            bhVacuum->set(false);
            throw ActionError("BondHead", tr("Pick dut failed! Vacuum did not detect feedback signal!"));
        }
        if (pickPlaceConfig->delayAfterOpenVacuum() > 0)
        {
            QThread::msleep(pickPlaceConfig->delayAfterOpenVacuum());
        }
    }
    catch (SilicoolException &se)
    {
        softLandUpTwoZ();
        throw se;
    }
}

void BonderHead::pickDutDownAction()
{
    if (pickPlaceConfig->useFastSoftlandingPick())
    {
        bhMiniZMotor->fastSoftLanding(pickPlaceConfig->softlandingPickVel(), bhMiniZMotor->positiveLimit());
    }
    else
    {
        double currentZ = bhMiniZMotor->getCurrentOutputPos();
        double margin = bhMiniZMotor->positiveLimit() - currentZ - 0.01;
        bhMiniZMotor->softLandDown(pickPlaceConfig->softlandingPickVel(), bhMiniZMotor->positiveLimit(),
                                   pickPlaceConfig->softlandingPickForce(), margin);
    }
}

void BonderHead::softlandUpMiniZWithoutWait()
{
    if (bhMiniZMotor->isSoftlandingDown())
    {
        bhMiniZMotor->softLandUp(false);
        QThread::msleep(pickPlaceConfig->delayAfterSoftLandUpCmd());
    }
    else
    {
        bhMiniZ->moveTo(SAxisPosName::StandbyPos, false);
        QThread::msleep(5);
    }
}

void BonderHead::checkDutAfterPick()
{
    if (!bhVacuum->get())
    {
        bhVacuum->set(false);
        throw ActionError("BondHead", tr("Pick dut failed! Vacuum lost feedback signal after bond head up!"));
    }
}

void BonderHead::placeDutDown()
{
    if (pickPlaceConfig->delayBeforePlace() > 0)
    {
        QThread::msleep(pickPlaceConfig->delayBeforePlace());
    }
    try
    {
        placeDutDownAction();
        if (pickPlaceConfig->delayAfterSoftlandContact() > 0)
        {
            QThread::msleep(pickPlaceConfig->delayAfterSoftlandContact());
        }
        bhVacuum->set(false);
        if (pickPlaceConfig->delayAfterCloseVacuum() > 0)
        {
            QThread::msleep(pickPlaceConfig->delayAfterCloseVacuum());
        }
    }
    catch (SilicoolException &se)
    {
        softLandUpTwoZ();
        throw se;
    }
}

void BonderHead::placeDutDownAction()
{
    if (pickPlaceConfig->useFastSoftlandingPlace())
    {
        bhMiniZMotor->fastSoftLanding(pickPlaceConfig->softlandingPlaceVel(), bhMiniZMotor->positiveLimit());
    }
    else
    {
        double currentZ = bhMiniZMotor->getCurrentOutputPos();
        double margin = bhMiniZMotor->positiveLimit() - currentZ - 0.01;
        bhMiniZMotor->softLandDown(pickPlaceConfig->softlandingPlaceVel(), bhMiniZMotor->positiveLimit(),
                                   pickPlaceConfig->softlandingPlaceForce(), margin);
    }
}

void BonderHead::softLandUpTwoZ()
{
    softlandUpMiniZWithoutWait();
    bhXyz->moveZToPos(XYZPosName::StandbyPos);
}

QPointF BonderHead::getCorToUplookCameraOffset()
{
    if (bhConfig->corCaliStepCount() < 3)
    {
        throw SilicolAbort(tr("COR calibration step count must greater than 2!"));
    }

    QVector<QPointF> offsets;
    //    bhThetaMotor->absMove(bhConfig->corCaliStartPos());
    //    QThread::msleep(400);
    for (int i = 0; i < bhConfig->corCaliStepCount(); i++)
    {
        PrOffset offset = getCaliGlassUplookPrOffset();
        offsets.append(QPointF(offset.X, offset.Y));
        if (i != bhConfig->corCaliStepCount() - 1)
        {
            bhThetaMotor->relMove(bhConfig->corCaliStepSize());
            QThread::msleep(200);
        }
    }

    QPointF nozzelCenter;
    double radius;
    fitCircle(offsets, nozzelCenter, radius);
    return nozzelCenter;
}

PrOffset BonderHead::getLTCaliGlassPrOffset()
{
    PrOffset prOffset;

    if (!qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
    {
        ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift());
    }

    bool res = ltCaliGlassLocation->performPR(prOffset);

    if (!qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
    {
        ltLoaderTrackX->relMove(bhConfig->ltCameraPrPosXShift() * -1);
    }

    if (!res)
    {
        throw SilicolAbort(tr("LTCaliGlassLocation perform pr failed!"));
    }

    prOffset.X -= bhConfig->ltCameraPrPosXShift();
    prOffset.O_X -= bhConfig->ltCameraPrPosXShift();

    return prOffset;
}

PrOffset BonderHead::getCaliGlassUplookPrOffset()
{
    PrOffset prOffset;
    if (!caliGlassUplookLocation->performPR(prOffset))
    {
        throw SilicolAbort(tr("CaliGlassUplookLocation perform pr failed!"));
    }
    return prOffset;
}

PrOffset BonderHead::getSTCaliGlassPrOffset()
{
    bwhOpticZ->moveTo(SAxisPosName::CaliGlassPrPos);
    PrOffset prOffset;
    if (!stCaliGlassLocation->performPR(prOffset))
    {
        throw SilicolAbort(tr("STCaliGlassLocation perform pr failed!"));
    }
    return prOffset;
}

PrOffset BonderHead::getLensUplookOffset()
{
    PrOffset prOffset;
    if (!getLensUplookOffsetImpl(prOffset))
    {
        throw SilicolAbort(tr("Lens uplook pr failed!"));
    }
    return prOffset;
}

PrOffset BonderHead::getLensUplookRectOffset()
{
    PrOffset prOffset;
    if (!lensUplookRectLocation->performPR(prOffset))
    {
        throw SilicolAbort(tr("Lens uplook rectangle pr failed!"));
    }
    return prOffset;
}

bool BonderHead::getLensUplookOffsetImpl(PrOffset &prOffset)
{
    prOffset.ReSet();
    if (bhConfig->alignCircularLensToSensor())
    {
        QImage image = lensUplookRectLocation->getImage();
        PrOffset xyOffset;
        PrOffset thetaOffset;
        if (lensUplookRectLocation->performPR(image, thetaOffset, false)
            && lensUplookCircleLocation->performPR(image, xyOffset, true))
        {
            prOffset.X = xyOffset.X;
            prOffset.Y = xyOffset.Y;
            prOffset.Theta = thetaOffset.Theta;
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        return lensUplookRectLocation->performPR(prOffset);
    }
}

void BonderHead::showDeviationTestResult(QString tipTitle,
                                         QString reportTitle,
                                         QList<double> &xOffsets,
                                         QList<double> &yOffsets,
                                         QList<double> &thetaOffsets)
{
    QFile logFile(QString("./deviationTestData/%1_%2.csv")
                      .arg(QDateTime::currentDateTime().toString("yyyy_MM_dd_hh_mm_ss_zzz"))
                      .arg(reportTitle));
    QDir dir("./deviationTestData");
    if (!dir.exists())
    {
        dir.mkpath(dir.absolutePath());
    }
    logFile.open(QIODevice::WriteOnly);
    logFile.write("XOffset,YOffset,ThetaOffset\r\n");
    for (int i = 0; i < xOffsets.count(); i++)
    {
        logFile.write(QString("%1,%2,%3\r\n").arg(xOffsets[i]).arg(yOffsets[i]).arg(thetaOffsets[i]).toUtf8());
    }
    logFile.flush();
    logFile.close();

    QString msg = tipTitle + "\r\n";
    msg += getMaxMinAveInfo("XOffset", xOffsets) + "\r\n";
    msg += getMaxMinAveInfo("YOffset", yOffsets) + "\r\n";
    msg += getMaxMinAveInfo("ThetaOffset", thetaOffsets);
    UIOperation::getIns()->showTip(msg);
}

QString BonderHead::getMaxMinAveInfo(QString title, QList<double> &values)
{
    double maxValue = max<double>(values);
    double minValue = min<double>(values);
    return QString("%1: max_%2, min_%3, range_%4").arg(title).arg(maxValue).arg(minValue).arg(maxValue - minValue);
}

void BonderHead::moveToPlaceCaliGlassToLTCameraPos(PrOffset &uplookOffset,
                                                   bool useCorFromCalibration,
                                                   double targetXOffset,
                                                   double targetYOffset,
                                                   double targetThetaOffset)
{
    double placeZHeight = bhXyz->getPos<XYZModulePos>(XYZPosName::LTPickPlaceCaliGlassPos)->zPos();
    if (useCorFromCalibration)
    {
        moveToPlaceDutToCameraPos(uplookOffset, bhConfig->ltCameraToUplookCameraOffset(), placeZHeight, true,
                                  targetXOffset, targetYOffset, targetThetaOffset);
    }
    else
    {
        double thetaAngle
            = targetThetaOffset - (bhConfig->ltCameraToUplookCameraOffset()->theta() - uplookOffset.Theta);
        bhThetaMotor->relMove(thetaAngle, true, 0.001);
        PrOffset offsetAfterRotation = getCaliGlassUplookPrOffset();
        moveToPlaceDutToCameraPos(offsetAfterRotation, bhConfig->ltCameraToUplookCameraOffset(), placeZHeight, false,
                                  targetXOffset, targetYOffset, targetThetaOffset);
    }
}

void BonderHead::moveToPlaceCaliGlassToSTCameraPos(PrOffset &uplookOffset,
                                                   CameraOffset *cameraToUplookCameraOffset,
                                                   bool useCorFromCalibration,
                                                   double targetXOffset,
                                                   double targetYOffset,
                                                   double targetThetaOffset,
                                                   TwoDimensionCalibration *tdc)
{
    double currentOpticYPos = bwhOpticXY->yAxis()->getFeedbackPos(-1);
    targetYOffset += currentOpticYPos;
    double placeZHeight = bhXyz->getPos<XYZModulePos>(XYZPosName::STPickPlaceCaliGlassPos)->zPos();
    if (useCorFromCalibration)
    {
        moveToPlaceDutToCameraPos(uplookOffset, cameraToUplookCameraOffset, placeZHeight, true, targetXOffset,
                                  targetYOffset, targetThetaOffset, tdc);
    }
    else
    {
        double thetaAngle = targetThetaOffset - (cameraToUplookCameraOffset->theta() - uplookOffset.Theta);
        bhThetaMotor->relMove(thetaAngle, true, 0.001);
        PrOffset offsetAfterRotation = getCaliGlassUplookPrOffset();
        moveToPlaceDutToCameraPos(offsetAfterRotation, cameraToUplookCameraOffset, placeZHeight, false, targetXOffset,
                                  targetYOffset, targetThetaOffset, tdc);
    }
}

void BonderHead::fastMoveToStCameraPlacePos(PrOffset &uplookOffset,
                                            CameraOffset *cameraToUplookCameraOffset,
                                            bool rotate,
                                            double targetXOffset,
                                            double targetYOffset,
                                            double targetThetaOffset,
                                            TwoDimensionCalibration *tdc)
{
    double prXOffset, prYOffset;
    auto uplookPos = bhXyz->getPos<XYZModulePos>(XYZPosName::UplookPos);
    if (rotate)
    {
        double thetaAngle = targetThetaOffset - (cameraToUplookCameraOffset->theta() - uplookOffset.Theta);
        double thetaRadian = thetaAngle * M_PI / 180;

        QPointF cor(uplookPos->xPos() + bhConfig->bhCorToUplookCameraOffsetX(),
                    uplookPos->yPos() + bhConfig->bhCorToUplookCameraOffsetY());
        QPointF dutCenter(uplookOffset.X, uplookOffset.Y);
        QPointF dutCenterToCorOffset = dutCenter - cor;
        prXOffset = dutCenterToCorOffset.x() * cos(thetaRadian) + dutCenterToCorOffset.y() * sin(thetaRadian) + cor.x();
        prYOffset
            = -dutCenterToCorOffset.x() * sin(thetaRadian) + dutCenterToCorOffset.y() * cos(thetaRadian) + cor.y();
        bhThetaMotor->relMove(thetaAngle, false);
    }
    else
    {
        prXOffset = uplookOffset.X;
        prYOffset = uplookOffset.Y;
    }

    QPointF targetXyPos(uplookPos->xPos() + cameraToUplookCameraOffset->x() - prXOffset + targetXOffset,
                        uplookPos->yPos() + cameraToUplookCameraOffset->y() - prYOffset + targetYOffset);
    if (tdc != nullptr)
    {
        targetXyPos = tdc->nnA2B(targetXyPos);
    }
    SingleAxisPos *targetZPos = bhZ->getPos<SingleAxisPos>(SAxisPosName::STPickPlacePos);
    bhXyz->xAxis()->absMove(targetXyPos.x(), false);
    bhXyz->yAxis()->absForceMove(targetXyPos.y());
    yzPlanner->moveTo(targetXyPos.y(), targetZPos->pos(), bhConfig->safetyPosNearST()->zPos(),
                      bhConfig->safetyPosNearST()->yPos(), TwoAxisPlanner::GreaterThan, false);
    bhXyz->xAxis()->waitArrivedPos();
    if (rotate)
    {
        bhThetaMotor->waitArrivedPos();
    }
}

void BonderHead::moveToPlaceDutToCameraPos(PrOffset &uplookOffset,
                                           CameraOffset *cameraToUplookCameraOffset,
                                           double readyForPlaceZHeight,
                                           bool rotate,
                                           double targetXOffset,
                                           double targetYOffset,
                                           double targetThetaOffset,
                                           TwoDimensionCalibration *tdc)
{
    double prXOffset, prYOffset;
    auto uplookPos = bhXyz->getPos<XYZModulePos>(XYZPosName::UplookPos);
    if (rotate)
    {
        double thetaAngle = targetThetaOffset - (cameraToUplookCameraOffset->theta() - uplookOffset.Theta);
        double thetaRadian = thetaAngle * M_PI / 180;

        QPointF cor(uplookPos->xPos() + bhConfig->bhCorToUplookCameraOffsetX(),
                    uplookPos->yPos() + bhConfig->bhCorToUplookCameraOffsetY());
        QPointF dutCenter(uplookOffset.X, uplookOffset.Y);
        QPointF dutCenterToCorOffset = dutCenter - cor;
        prXOffset = dutCenterToCorOffset.x() * cos(thetaRadian) + dutCenterToCorOffset.y() * sin(thetaRadian) + cor.x();
        prYOffset
            = -dutCenterToCorOffset.x() * sin(thetaRadian) + dutCenterToCorOffset.y() * cos(thetaRadian) + cor.y();
        bhThetaMotor->relMove(thetaAngle, false);
    }
    else
    {
        prXOffset = uplookOffset.X;
        prYOffset = uplookOffset.Y;
    }

    QPointF targetXyPos(uplookPos->xPos() + cameraToUplookCameraOffset->x() - prXOffset + targetXOffset,
                        uplookPos->yPos() + cameraToUplookCameraOffset->y() - prYOffset + targetYOffset);
    if (tdc != nullptr)
    {
        targetXyPos = tdc->nnA2B(targetXyPos);
    }
    bhXyz->moveXY(targetXyPos.x(), targetXyPos.y());
    bhXyz->zAxis()->absMove(readyForPlaceZHeight);
    if (rotate)
    {
        bhThetaMotor->waitArrivedPos();
    }
}

void BonderHead::moveToPickSTCaliGlassPos(double targetXOffset, double targetYOffset)
{
    bhTheta->moveTo(SAxisPosName::StandbyPos, false);
    PrOffset prOffset = getSTCaliGlassPrOffset();
    double currentBWHOpticYPos = bwhOpticXY->yAxis()->getFeedbackPos(-1);
    bhXyz->moveXY(bhConfig->bhToSTCameraOffsetX() + prOffset.X + targetXOffset,
                  bhConfig->bhToSTCameraOffsetY() + prOffset.Y + currentBWHOpticYPos + targetYOffset);
    bhXyz->moveZToPos(XYZPosName::STPickPlaceCaliGlassPos);
    bhTheta->waitArrivedPos(SAxisPosName::StandbyPos);
}

double BonderHead::getRand(double min, double max)
{
    const int precision = 1000;

    int range = (max - min) * precision;
    return rand() % range * 1.0 / precision + min;
}

void BonderHead::verifyTdcParam(TwoDimensionCaliParameter *tdcParam)
{
    if (tdcParam->xStartAbsPos() > tdcParam->xEndAbsPos())
    {
        double xStart = tdcParam->xStartAbsPos();
        tdcParam->setXStartAbsPos(tdcParam->xEndAbsPos());
        tdcParam->setXEndAbsPos(xStart);
    }
    if (tdcParam->yStartRelPos() > tdcParam->yEndRelPos())
    {
        double yStart = tdcParam->yStartRelPos();
        tdcParam->setYStartRelPos(tdcParam->yEndRelPos());
        tdcParam->setYEndRelPos(yStart);
    }
}

void BonderHead::doTwoDimensionCalibration(bool isSensorTraySide, int sensorTraySideRowIndex)
{
    CameraOffset *cameraOffset
        = isSensorTraySide
              ? bhConfig->stCameraToUplookCameraOffsetOfEachRow()->getConfig<CameraOffset>(sensorTraySideRowIndex)
              : bhConfig->ltCameraToUplookCameraOffset();
    TwoDimensionCaliParameter *tdcParam = isSensorTraySide ? bhConfig->stSideTwoDimensionCalibrationParam()
                                                           : bhConfig->ltSideTwoDimensionCalibrationParam();
    verifyTdcParam(tdcParam);
    TwoDimensionCalibration *tdc = isSensorTraySide ? stSideTdc(sensorTraySideRowIndex) : ltSideTdc;
    auto uplookPos = bhXyz->getPos<XYZModulePos>(XYZPosName::UplookPos);

    bool gotIt = false;

    for (int i = 0; i < tdcParam->referencePointReachPrecisionMaxRetryTimes(); i++)
    {
        if (isSensorTraySide)
        {
            moveToPickSTCaliGlass();
            bwhOpticXY->yAxis()->absMove(bwh->getBWHOpticYPrPos(sensorTraySideRowIndex));
        }
        else
        {
            moveToPickLTCaliGlass();
        }

        bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::XYZ);
        QThread::msleep(200);
        PrOffset uplookOffset = getCaliGlassUplookPrOffset();

        if (isSensorTraySide)
        {
            moveToPlaceCaliGlassToSTCameraPos(uplookOffset, cameraOffset, false, bhConfig->stCameraCenterXShift() * -1);
        }
        else
        {
            moveToPlaceCaliGlassToLTCameraPos(uplookOffset, false, bhConfig->ltCameraCenterXShift() * -1);
        }

        placeDut(true);
        double placeDutBhXPos = bhXyz->xAxis()->getFeedbackPos(-1);
        double placeDutBhYPos = bhXyz->yAxis()->getFeedbackPos(-1);
        bhXyz->moveXYToPos(XYZPosName::StandbyPos);

        PrOffset downlookOffset = isSensorTraySide ? getSTCaliGlassPrOffset() : getLTCaliGlassPrOffset();
        double placeShift = isSensorTraySide ? bhConfig->stCameraCenterXShift() : bhConfig->ltCameraCenterXShift();
        if (qAbs(downlookOffset.Y) < qAbs(tdcParam->referencePointPrecisionY())
            && qAbs(downlookOffset.X + placeShift) < qAbs(tdcParam->referencePointPrecisionX()))
        {
            gotIt = true;
            break;
        }

        double offsetX = placeDutBhXPos - uplookPos->xPos() - downlookOffset.X + uplookOffset.X;
        double offsetY = placeDutBhYPos - uplookPos->yPos() - downlookOffset.Y + uplookOffset.Y;
        if (isSensorTraySide)
        {
            offsetY -= bwh->getBWHOpticYPrPos(sensorTraySideRowIndex);
        }
        double offsetTheta = downlookOffset.Theta + uplookOffset.Theta;
        cameraOffset->setX(offsetX);
        cameraOffset->setY(offsetY);
        cameraOffset->setTheta(offsetTheta);
    }
    if (!gotIt)
    {
        QString errMsg
            = isSensorTraySide
                  ? tr("Sensor tray place precision can not meet limit! RowIndex: %1").arg(sensorTraySideRowIndex)
                  : tr("Lens tray place precision can not meet limit!");
        throw SilicolAbort(errMsg);
    }

    double xStepSize = (tdcParam->xEndAbsPos() - tdcParam->xStartAbsPos()) / (tdcParam->xStep() - 1);
    double yStepSize = (tdcParam->yEndRelPos() - tdcParam->yStartRelPos()) / (tdcParam->yStep() - 1);
    QList<QPointF> motorPoints;     // 电机坐标系中的位置
    QList<QPointF> cameraPoints;    // 以相机为参考的坐标系中的位置
    for (int rowIndex = 0; rowIndex < tdcParam->yStep(); rowIndex++)
    {
        for (int columnIndex = 0; columnIndex < tdcParam->xStep(); columnIndex++)
        {
            if (isSensorTraySide)
            {
                moveToPickSTCaliGlass();
            }
            else
            {
                moveToPickLTCaliGlass();
            }

            bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::XYZ);
            QThread::msleep(200);
            PrOffset uplookOffset = getCaliGlassUplookPrOffset();
            double thetaAngle = -(cameraOffset->theta() - uplookOffset.Theta);
            bhThetaMotor->relMove(thetaAngle, true, 0.001);
            uplookOffset = getCaliGlassUplookPrOffset();

            QPointF placeToCenterPos(uplookPos->xPos() + cameraOffset->x() - uplookOffset.X,
                                     uplookPos->yPos() + cameraOffset->y() - uplookOffset.Y);
            if (isSensorTraySide)
            {
                placeToCenterPos.setY(placeToCenterPos.y() + bwh->getBWHOpticYPrPos(sensorTraySideRowIndex));
            }
            QPointF motorPos(tdcParam->xStartAbsPos() + xStepSize * columnIndex,
                             placeToCenterPos.y() + tdcParam->yStartRelPos() + yStepSize * rowIndex);
            bhXyz->moveXY(motorPos.x(), motorPos.y());

            if (isSensorTraySide)
            {
                bhXyz->moveZToPos(XYZPosName::STPickPlaceCaliGlassPos);
            }
            else
            {
                bhXyz->moveZToPos(XYZPosName::LTPickPlaceCaliGlassPos);
            }

            placeDut(true);
            bhXyz->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
            PrOffset downlookOffset = isSensorTraySide ? getSTCaliGlassPrOffset() : getLTCaliGlassPrOffset();
            QPointF cameraPos = placeToCenterPos + QPointF(downlookOffset.X, downlookOffset.Y);
            motorPoints.append(motorPos);
            cameraPoints.append(cameraPos);
        }
    }
    tdc->setData(tdcParam->xStep(), tdcParam->yStep(), cameraPoints, motorPoints);
}

QString BonderHead::stSideTdcFileName(int rowIndex) const
{
    return QString("%1/stSideTdcRow%2.tdc").arg(getWorkerConfigDir()).arg(rowIndex);
}

QString BonderHead::ltSideTdcFileName() const
{
    return "./config/motionConfig/lensTraySideTdc.tdc";
}

void BonderHead::freeTdcDatas()
{
    while (stSideTdcs.count() > 0)
    {
        auto tdc = stSideTdcs.takeFirst();
        delete tdc;
    }
    if (ltSideTdc != nullptr)
    {
        delete ltSideTdc;
        ltSideTdc = nullptr;
    }
}

void BonderHead::loadTdcDatas()
{
    for (int i = 0; i < sensorTrayConfig->rows(); i++)
    {
        if (stSideTdcs.count() <= i)
        {
            stSideTdcs.append(new TwoDimensionCalibration());
        }
        stSideTdcs[i]->load(stSideTdcFileName(i));
    }
    if (ltSideTdc == nullptr)
    {
        ltSideTdc = new TwoDimensionCalibration();
    }
    ltSideTdc->load(ltSideTdcFileName());
}

TwoDimensionCalibration *BonderHead::stSideTdc(int rowIndex)
{
    if (stSideTdcs.count() <= rowIndex)
    {
        throw SilicolAbort(tr("Lost two dimension calibration data of row index: %1").arg(rowIndex), EX_LOCATION);
    }
    return stSideTdcs[rowIndex];
}

void BonderHead::checkLensIndex()
{
    if (ltLoader->getLastLensIndex().x() == -1)
    {
        throw SilicolAbort(tr("Logic error! Lens index was [-1,-1]!"), EX_LOCATION);
    }
}

void BonderHead::checkSensorIndex()
{
    if (bwh->getLastSensorIndex().x() == -1)
    {
        throw SilicolAbort(tr("Logic error! Sensor index was [-1,-1]!"), EX_LOCATION);
    }
}

void BonderHead::pushPickLensEvent()
{
    pushGetLensEventToLTL();
    pushEvent(WorkerEvent("waitLens"));
    pushEvent(WorkerEvent("directMoveToPickLensPos"));
    pushEvent(WorkerEvent("pickLens"));
}

void BonderHead::pushGetLensEventToLTL()
{
    ltLoader->resetGotLens();
    ltLoader->pushEvent(WorkerEvent("getLens"));
}

void BonderHead::pushGetSensorEventToBWH()
{
    bwh->resetGotSensor();
    bwh->pushEvent(WorkerEvent("getSensor"));
}

void BonderHead::handleUplookPrFailed()
{
    bhXyz->yAxis()->waitArrivedPos();
    bhXyz->moveToPos(XYZPosName::UplookPos, XYZModulePos::Z_XY);
    LensUplookPrError error;
    auto option = ErrorHandler::getErrorHandlingOption(error);
    if (option == Retry_)
    {
        pushEvent(WorkerEvent("performUplookPrAndMoveToBondPos"));
        return;
    }
    else if (option == ManuallyTakeAway_)
    {
        bhVacuum->set(false);
        ltLoader->setLensState(pickedLensIndex, LensTrayMaterialData::UplookFailed);
        pushPickLensEvent();
        return;
    }
    else
    {
        ltLoader->setLensState(pickedLensIndex, LensTrayMaterialData::UplookFailed);
        throw SilicolAbort("User selected abort!");
    }
}

void BonderHead::handleStop()
{
    clearEvent();
    moveToStandbyPos(true);
    stopEventHandled();
    emit sendEvent(WorkerName::LensTrayMagazine, WorkerEvent("bhStopped"));
}

void BonderHead::directMoveToPickLensPosImpl(PrOffset &lensPrOffset)
{
    QPointF targetXyPos(bhConfig->bhToLTCameraOffsetX() + lensPrOffset.X,
                        bhConfig->bhToLTCameraOffsetY() + lensPrOffset.Y);
    if (bhConfig->useLtSideTwoDimCaliData())
    {
        targetXyPos = ltSideTdc->nnA2B(targetXyPos);
    }
    bhXyz->xAxis()->absMove(targetXyPos.x(), false);
    SingleAxisPos *ltPickPlacePos = bhZ->getPos<SingleAxisPos>(SAxisPosName::LTPickPlacePos);
    yzPlanner->moveTo(targetXyPos.y(), ltPickPlacePos->pos(), bhConfig->safetyPosNearLT()->zPos(),
                      bhConfig->safetyPosNearLT()->yPos(), TwoAxisPlanner::LessThan);
    bhXyz->xAxis()->waitArrivedPos();
}

void BonderHead::directMoveToPlacebackLensPos(PrOffset &lensPrOffset)
{
    QPointF targetXyPos(bhConfig->bhToLTCameraOffsetX() + lensPrOffset.X,
                        bhConfig->bhToLTCameraOffsetY() + lensPrOffset.Y);
    if (bhConfig->useLtSideTwoDimCaliData())
    {
        targetXyPos = ltSideTdc->nnA2B(targetXyPos);
    }
    bhXyz->xAxis()->absMove(targetXyPos.x(), false);
    auto *uplookPos = bhXyz->getPos<XYZModulePos>(XYZPosName::UplookPos);
    double safetyHeightForPlacebackLens = qMin(bhConfig->ltSafetyHeightAfterPickLens(), uplookPos->zPos());
    yzPlanner->moveTo(targetXyPos.y(), safetyHeightForPlacebackLens, bhConfig->safetyPosNearLT()->zPos(),
                      bhConfig->safetyPosNearLT()->yPos(), TwoAxisPlanner::LessThan);
    bhXyz->xAxis()->waitArrivedPos();
    bhZ->moveTo(SAxisPosName::LTPickPlacePos);
}

void BonderHead::moveFromLtToUplookPosImpl()
{
    auto *uplookPos = bhXyz->getPos<XYZModulePos>(XYZPosName::UplookPos);
    bhXyz->zAxis()->waitGreaterThanPos(qMin(bhConfig->ltSafetyHeightAfterPickLens(), uplookPos->zPos()));
    bhXyz->xAxis()->absMove(uplookPos->xPos(), false);
    zyPlanner->moveTo(uplookPos->zPos(), uplookPos->yPos(), bhConfig->safetyPosNearLT()->yPos(),
                      bhConfig->safetyPosNearLT()->zPos(), TwoAxisPlanner::GreaterThan, false);
    bhXyz->xAxis()->waitArrivedPos();
}

void BonderHead::moveFromStToStandbyPosImpl()
{
    auto *standbyPos = bhXyz->getPos<XYZModulePos>(XYZPosName::StandbyPos);
    bhXyz->zAxis()->waitGreaterThanPos(qMin(bhConfig->stSafetyHeightAfterPlaceLens(), standbyPos->zPos()));
    bhXyz->xAxis()->absMove(standbyPos->xPos(), false);
    zyPlanner->moveTo(standbyPos->zPos(), standbyPos->yPos(), bhConfig->safetyPosNearST()->yPos(),
                      bhConfig->safetyPosNearST()->zPos(), TwoAxisPlanner::GreaterThan, false);
    bhXyz->xAxis()->waitArrivedPos();
}

void BonderHead::setHigherBHYZAcc()
{
    bhXyz->yAxis()->scaleMaxAcc(bhConfig->higherBHYAccRatio());
    bhXyz->zAxis()->scaleMaxAcc(bhConfig->higherBHZAccRatio());
}

void BonderHead::resetBHYZAcc()
{
    checkIsInit();

    bhXyz->yAxis()->resetMaxAcc();
    bhXyz->zAxis()->resetMaxAcc();
}
